
/**
 * @file SppFilter.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//  shjzhang
//
//  2020/01/16
//  remove stateMap/covMap, which makes the program complicated
//  shjzhang.
//
//============================================================================


#include "SppFilter.hpp"
#include "SystemTime.hpp"
#include "Ambiguity.hpp"
#include "NEUUtil.hpp"

#define debug 0

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int SppFilter::classIndex = 9600000;

      // Returns an index identifying this object.
   int SppFilter::getIndex() const
   { return index; }

      // Returns a string identifying this object.
   std::string SppFilter::getClassName() const
   { return "SppFilter"; }

   void SppFilter::Init(void)
   {
       // set up variables and equations for PPP

       // Warning: not consider the GLONASS system until now
       // Warning: only consider LC combinations
       // Warning: see SppFilterUC for UC combinations

       //////////////////////////////////////////////////////////////
       //
       // please augument for GAL/BDS system
       // 
       // wavelengthBLC12 = clight/(f1+f2)
       //
       // BLC12  = clight/(f1+f2)[( B1 + f2/(f1-f2)*Nw]
       // BLC15  = clight/(f1+f5)[( B1 + f5/(f1-f5)*Nw)]
       //
       //////////////////////////////////////////////////////////////

   }

   void SppFilter::setUpEquations(void)
   {

       ///////////////////////////////////////////////
       //
       // Now, define variables need to be estimated 
       // 
       ///////////////////////////////////////////////
       
          // These variables are, by default, SourceID-indexed
       Variable dx( TypeID::dStaX );
       Variable dy( TypeID::dStaY );
       Variable dz( TypeID::dStaZ );

       VariableSet xyzVarSet;
       xyzVarSet.insert(dx);
       xyzVarSet.insert(dy);
       xyzVarSet.insert(dz);

       Variable dt( TypeID::dcdtSta );
       Variable dtGAL( TypeID::dcdtStaGAL );
       Variable dtBDS( TypeID::dcdtStaBDS );

       //////////////////////////////
       // Define Equation object for code/phase observations, and add variables 
       //////////////////////////////

       // GPS LC
       Equation equPC12GPS( TypeID::prefitC12 );
       Equation equPC15GPS( TypeID::prefitC15 );

       // GAL LC
       Equation equPC15GAL( TypeID::prefitC15, SatelliteSystem::Galileo );
       Equation equPC17GAL( TypeID::prefitC17, SatelliteSystem::Galileo );
       Equation equPC18GAL( TypeID::prefitC18, SatelliteSystem::Galileo );
       Equation equPC16GAL( TypeID::prefitC16, SatelliteSystem::Galileo );

       // BDS LC                    
       Equation equPC27BDS( TypeID::prefitC27, SatelliteSystem::BDS );
       Equation equPC26BDS( TypeID::prefitC26, SatelliteSystem::BDS );

       /////////////////////////
       // insert variables into equations
       /////////////////////////

       // GPS LC
       equPC12GPS.addVarSet(xyzVarSet); 
       equPC15GPS.addVarSet(xyzVarSet); 

       // GAL LC
       equPC15GAL.addVarSet(xyzVarSet); 
       equPC17GAL.addVarSet(xyzVarSet); 
       equPC18GAL.addVarSet(xyzVarSet); 
       equPC16GAL.addVarSet(xyzVarSet); 

       // BDS LC
       equPC27BDS.addVarSet(xyzVarSet); 
       equPC26BDS.addVarSet(xyzVarSet); 

       // Insert dt 
       // GPS
       equPC12GPS.addVariable(dt, true);  
       equPC15GPS.addVariable(dt, true);  

       // GAL
       equPC15GAL.addVariable(dtGAL, true); 
       equPC17GAL.addVariable(dtGAL, true); 
       equPC18GAL.addVariable(dtGAL, true); 
       equPC16GAL.addVariable(dtGAL, true); 

       // BDS LC
       equPC27BDS.addVariable(dtBDS, true); 
       equPC26BDS.addVariable(dtBDS, true); 

       // define the weight for all combinations relative PC12GPS

          // Phase equations should have higher relative weight
       equPC12GPS.setWeight(1.0);              
       equPC15GPS.setWeight(1.0);                     

       equPC15GAL.setWeight(1.0); 
       equPC17GAL.setWeight(1.0); 
       equPC18GAL.setWeight(1.0); 
       equPC16GAL.setWeight(1.0); 

       equPC27BDS.setWeight(1.0); 
       equPC26BDS.setWeight(1.0); 

          ///////////
          // SETTING THE RULES: SETUP EQUATION SYSTEM
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       
          // Add equation descriptions
       if(usingGPS)
       {
           for(TypeIDSet::const_iterator typeIt=gpsCombTypeSet.begin();
                   typeIt!=gpsCombTypeSet.end();
                   typeIt++)
           {
               if( (*typeIt) == TypeID::PC12 )
               {
                   equSystem.addEquation(equPC12GPS);
               }
               else if ( (*typeIt) == TypeID::PC15 )
               {
                   equSystem.addEquation(equPC15GPS);
               }
           }
       }

       if(usingGAL)
       {
           for(TypeIDSet::const_iterator typeIt=galCombTypeSet.begin();
                   typeIt!=galCombTypeSet.end();
                   typeIt++)
           {
               if ( (*typeIt) == TypeID::PC15 )
               {
                   equSystem.addEquation(equPC15GAL);
               }
               else if ( (*typeIt) == TypeID::PC17 )
               {
                   equSystem.addEquation(equPC17GAL);
               }
               else if ( (*typeIt) == TypeID::PC18 )
               {
                   equSystem.addEquation(equPC18GAL);
               }
               else if ( (*typeIt) == TypeID::PC16 )
               {
                   equSystem.addEquation(equPC16GAL);
               }
           }
       }

       if(usingBDS)
       {
           for(TypeIDSet::const_iterator typeIt=bdsCombTypeSet.begin();
                   typeIt!=bdsCombTypeSet.end();
                   typeIt++)
           {
               if( (*typeIt) == TypeID::PC27 )
               {
                   equSystem.addEquation(equPC27BDS);
               }
               else if ( (*typeIt) == TypeID::PC26 )
               {
                   equSystem.addEquation(equPC26BDS);
               }
           }
       }

   } // end of setUpEquations



      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SppFilter::Process( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {
          if(debug)
          {
              cout << "SppFilter" << endl;
          }

          // set-up equtions firstly for equation systems
          setUpEquations();

          // let's compute the time-difference data for cycle-slip detection
          equSystem.Prepare(gRin);

          if(debug)
             gRin.dump(cout, 1);

             // Get the number of unknowns being processed
          int numEqu( equSystem.getEquNumber() );
          int numUnknowns( equSystem.getTotalNumVariables() );


             // Get the set with unknowns being processed
          currentUnkSet =  equSystem.getVarUnknowns() ;

          std::list<Equation> currentEquList;
          currentEquList = equSystem.getCurrentEquationsList();

          if(debug)
          {
             for(std::list<Equation>::iterator it=currentEquList.begin();
                 it!=currentEquList.end();
                 ++it)
             {
                 (*it).dump(cout);
             }
          }

          // Get matrices and vectors out of equation system
          // Measurements vector (Prefit-residuals)
          measVector = equSystem.getPrefitsVector();
          hMatrix = equSystem.getGeometryMatrix();
          rMatrix = equSystem.getWeightsMatrix();


          /////////////////////////
          // apriori coordinate constraint
          /////////////////////////

          // if 1, means cycle slip happened, else it's good one
          // warning, 
          // the dimension of outlierVec is equal with numEqu, don't include the
          // extended constraint equation
          Vector<int> outlierVec(numEqu, 0);

          Vector<double> postfitResiduals;
          Vector<double> standResidual;
          double sig;

          if(firstTime)
          {
              vtpvAcc = 0.0;
              numObsAcc = 0.0;
              firstTime = false;
              return gRin;
          }

          // detect outlier using modified DIA method
          if(numEqu > numUnknowns)
          {
             DIA( measVector, 
                  hMatrix, 
                  rMatrix, 
                  postfitResiduals, 
                  standResidual, 
                  sig,
                  outlierVec); 
          }

          if(debug)
          {
              cout << "measVector" << endl;
              cout << measVector << endl;

              cout << "postfitResiduals" << endl;
              cout << postfitResiduals<< endl;

              cout << "cs:" << endl;
              cout << outlierVec << endl;

              cout << "solution" << endl;
              cout << solution << endl;
          }
          else
          {
              return gRin;
          }

          int i(0);
          for(auto equ: currentEquList)
          {
              SatID equSat = equ.header.equationSat;

              // insert cycle slip flags into data 
              if(outlierVec(i) == 1)
              {
                  gRin.body.removeSatID(equSat);
              }

              // index
              i++;
          }

          // reset filter
          CommonTime epoch( gRin.header.epoch );

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SppFilter::preCompute()'


      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SppFilter::DIA( Vector<double>& prefitResiduals,
                       Matrix<double>& designMatrix,
                       Matrix<double>& weightMatrix,
                       Vector<double>& postfitResiduals,
                       Vector<double>& standResidual,
                       double& sig,
                       Vector<int>& outlierVector)
       noexcept(false)
   {

         // By default, results are invalid
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("Compute(): Weight matrix is not square");
         cerr << e << endl;
         exit(-1);
      }

      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match!");
         cerr << e << endl;
         exit(-1);
      }

      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match !");
         cerr << e << endl;
         exit(-1);
      }

         // After checking sizes, let's do the real correction work
      Matrix<double> AT( transpose(designMatrix) );

      int numEqu( equSystem.getEquNumber() );
      int numUnknowns( equSystem.getTotalNumVariables() );

      // Covariance for residual V;
      Matrix<double> Qvv(pRow, pRow, 0.0);

      Matrix<double> tempWeight = weightMatrix;
      Vector<int>    outlierVec(numEqu, 0);

      Vector<double> stdResidual(pRow, 0.0);
      Vector<double> robustResidual(pRow, 0.0);

      double VTPV;
      int numValid = numEqu;

      double maxResidual;
      int iter = 0;
      double sigma;

      // one percent threshod for standard normal distribution
      double threshold = 8 ; 


      do 
      {
          if(debug)
          {
              cout << "AT:" << endl;
              cout << AT << endl;
              cout << "weight" << endl;
              cout << tempWeight << endl;
          }

          try
          {
             Matrix<double> invTemp;
             invTemp =  AT * tempWeight* designMatrix;

                // Compute the a posteriori error covariance matrix
             covMatrix = inverseChol( invTemp );

          }
          catch(...)
          {
             InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
             GPSTK_THROW(e);
             return -1;
          }

          try
          {
                // Compute the a posteriori state estimation
              solution = covMatrix * ( AT * tempWeight * prefitResiduals ) ;
          }
          catch(Exception e)
          {
             InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
             GPSTK_THROW(eis);
             return -1;
          }

             // Compute the postfit residuals Vector
          postfitResiduals = prefitResiduals - (designMatrix * solution);


          VTPV = 0.0;
          for( int i = 0; i < pRow; ++i)
          {
              VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
          }

          // update the weight
          // warning, the numvalid should be valid number
          sigma = std::sqrt( (VTPV)/(numValid - numUnknowns) );


          // let's compute the population variance
          double tempVTPVAcc;
          double tempNumObsAcc;

          tempVTPVAcc = vtpvAcc + VTPV;
          tempNumObsAcc = numObsAcc + numValid - numUnknowns;

          double totalSigma = std::sqrt(tempVTPVAcc/(tempNumObsAcc));


          Qvv = inverseChol(tempWeight) - designMatrix * covMatrix * AT;


          for(int i=0; i<pRow; i++)
          {
              stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(std::abs(Qvv(i,i)))) ) ;
          }

          for(int i=0; i<pRow; i++)
          {
              robustResidual[i] =  std::abs( postfitResiduals(i) /(totalSigma*std::sqrt(Qvv(i,i))) ) ;
          }

          int indexMax = getMaxIndex(stdResidual, outlierVec);

          if(ZTest)
          {
              maxResidual = robustResidual(indexMax);
          }
          else 
          {
              maxResidual = stdResidual(indexMax);
          }

          // apply a very small value for the outlier
          if( maxResidual > threshold )
          {
              tempWeight(indexMax, indexMax) = 1.0E-10;

              // update the flag for the outlier
              outlierVec(indexMax) = 1;

              // each time, one outlier will be removed
              numValid--;
          }

          iter++;

      } while( maxResidual > threshold && numValid > (numUnknowns) );

      // at last, solve the solution/covMatrix at last
      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix );

            // Compute the a posteriori error covariance matrix
         covMatrix = inverseChol( invTemp );

      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
          solution = covMatrix * ( AT * tempWeight * prefitResiduals );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      // update the weight
      // warning, the numvalid should be valid number
      sigma = std::sqrt( (VTPV )/(numValid - numUnknowns) );

      // let's compute the population variance
      double tempVTPVAcc;
      double tempNumObsAcc;

      tempVTPVAcc = vtpvAcc + VTPV;
      tempNumObsAcc = numObsAcc + numValid - numUnknowns;

      double totalSigma = std::sqrt(tempVTPVAcc/(tempNumObsAcc));

      Qvv = inverseChol(tempWeight) - designMatrix * covMatrix * AT;

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(std::abs(Qvv(i,i)))) ) ;
      }

      for(int i=0; i<pRow; i++)
      {
          robustResidual[i] =  std::abs( postfitResiduals(i) /(totalSigma*std::sqrt(Qvv(i,i))) ) ;
      }

      ///////////////////////
      // return
      ///////////////////////

      // local sigma and standResidual
      sig = sigma;
      standResidual = stdResidual;

      // store the total number of the freedom
      vtpvAcc += VTPV;
      numObsAcc += numValid - numUnknowns;

      // return the outlier flags
      outlierVector = outlierVec;

      return 0;

   }  // End of method 'SppFilter::MeasUpdate()'


   int SppFilter::getMaxIndex(Vector<double>& residual, 
                              Vector<int>& flagVec)
   {
       double max=0.0;
       int indexMax = 0;
       for(int i=0; i<residual.size(); i++)
       {
           if(std::abs(residual(i))>max && flagVec(i) !=1 )
           {
               max = std::abs(residual(i));
               indexMax = i;
           }
       }
       return indexMax;
   };


   int SppFilter::getMinIndex(Vector<double>& residual, 
                                 Vector<int>& flagVec)
   {
       double min= 1.0E+8;
       int minIndex = 0;
       for(int i=0; i<residual.size(); i++)
       {
           if(std::abs(residual(i)) < min && flagVec(i) !=1 )
           {
               min = std::abs(residual(i));
               minIndex = i;
           }
       }
       return minIndex;
   };

   Variable SppFilter::getVariable( const TypeID& type,
                                       const VariableSet& varSet) const
      noexcept(false)
   {

         // Declare an iterator for 'stateMap' and go to the first element
      VariableSet::const_iterator it = varSet.begin();

         // Look for a variable with the same type
      while( (*it).getType() != type &&
             it != varSet.end() )
      {
         ++it;

         // If the same type is not found, throw an exception
         if( it == varSet.end() )
         {
             InvalidRequest e("Type not found in varSet.");
             GPSTK_THROW(e);
         }
      }

         // Else, return the corresponding value
      return (*it);

   }  // End of method 'SolverGeneral::getVarialbe()'

       /* Returns the solution associated to a given TypeID.
       *
       * @param type    TypeID of the solution we are looking for.
       *
       * \warning In the case the solution contains more than one variable
       * of this type, only the first one will be returned.
       */
   double SppFilter::getSolution( const TypeID& type,
                                  const Vector<double>& stateVec ) 
      noexcept(false)
   {
         // Declare an varIterator for 'stateMap' and go to the first element
      auto varIt = currentUnkSet.begin();
      while( (*varIt).getType() != type )
      {
         // If the same type is not found, throw an exception
         if( varIt == currentUnkSet.end() )
         {
             InvalidRequest e("Type not found in solution vector.");
             cerr << e << endl;
             exit(-1);
         }

         varIt++;
      }

         // Else, return the corresponding value
      return stateVec( (*varIt).getNowIndex() );

   }  // End of method 'SolverGeneral::getSolution()'



}  // End of namespace gpstk
